#include "layered_costmap.h"


namespace costmap_2d{

LayeredCostmap::LayeredCostmap(std::string global_frame, std::string base_frame, bool rolling_window)
    :costmap_2d_(),
    global_frame_(global_frame),
    robot_base_frame_(base_frame),
    rolling_window_(rolling_window){
    
}

LayeredCostmap::~LayeredCostmap(){
    while(plugins_.size() > 0){
        plugins_.pop_back();
    }
}

void LayeredCostmap::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y){
    *min_x = std::min(x, *min_x);
    *min_y = std::min(y, *min_y);
    *max_x = std::max(x, *max_x);
    *max_y = std::max(y, *max_y);
}

void LayeredCostmap::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
//   if (!enabled_)
//     return;

//   unsigned char* master_array = master_grid.getCharMap();
//   unsigned int span = master_grid.getSizeInCellsX();

//   for (int j = min_j; j < max_j; j++)
//   {
//     unsigned int it = j * span + min_i;
//     for (int i = min_i; i < max_i; i++)
//     {
//       if (costmap_[it] == NO_INFORMATION){
//         it++;
//         continue;
//       }

//       unsigned char old_cost = master_array[it];
//       if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
//         master_array[it] = costmap_[it];
//       it++;
//     }
//   }
}

void LayeredCostmap::addPlugin(boost::shared_ptr<Layer> plugin){
    plugins_.push_back(plugin);
    LOG(INFO) << "addPlugin, size: " << plugins_.size() << endl;
}

std::vector<boost::shared_ptr<Layer> >* LayeredCostmap::getPlugins(){
    return &plugins_;
}

void LayeredCostmap::resizeMap(unsigned int width, unsigned int height, double resolution, double origin_x,
    double origin_y){
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_2d_.getMutex()));
    costmap_2d_.resizeMap(width, height, resolution, origin_x, origin_y);
    // LOG(INFO) << "resizeMap...... plugins_ size: " << plugins_.size() << endl;
    for(vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
        ++plugin){
        // LOG(INFO) << "resizeMap -->" << endl;
        (*plugin)->matchSize();
    }    
}

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw){
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_2d_.getMutex()));

    if(plugins_.empty())
        return;

    if (rolling_window_){
        double new_origin_x = robot_x - costmap_2d_.getWidthInMeters() / 2;
        double new_origin_y = robot_y - costmap_2d_.getHeightInMeters() / 2;
        costmap_2d_.updateOrigin(new_origin_x, new_origin_y);
    }

    minx_ = miny_ = 1e30;
    maxx_ = maxy_ = -1e30;
    // LOG(INFO) << "updateMap...... updateBounds" << endl; 
    for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin){
        (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    };

    // LOG(INFO) << "[updateBounds] min_x: " << minx_ << ", min_y: " << miny_ << ", max_x: " << maxx_ << ", max_y: " << maxy_;

    int x0, xn, y0, yn;
    costmap_2d_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
    costmap_2d_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
  
    x0 = std::max(0, x0);
    xn = std::min(int(costmap_2d_.getWidth()), xn + 1);
    y0 = std::max(0, y0);
    yn = std::min(int(costmap_2d_.getHeight()), yn + 1);
  
    // ROS_INFO("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
    // LOG(INFO) << "updateMap...... updateCosts" << endl; 

    costmap_2d_.resetMap(x0, y0, xn, yn);
    for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin){
        (*plugin)->updateCosts(costmap_2d_, x0, y0, xn, yn);
    };

}

Costmap2D* LayeredCostmap::getCostmap2D(){
    return &costmap_2d_;
}            

bool LayeredCostmap::isRolling(){
    return rolling_window_;
}

string LayeredCostmap::getGlobalFrameID(){
    return global_frame_;
}

string LayeredCostmap::getBaseFrameID(){
    return robot_base_frame_;   
}

}   // namespace costmap_2d
